#include "contiki.h"
#include "includes.h"
#include "driver/leds.h"
#include "driver/mpu6050.h"
PROCESS(main_process, "main process");
AUTOSTART_PROCESSES(&main_process);

extern mpu6050_t g_mpu6050;

PROCESS_THREAD(main_process, ev, data)
{
  static struct etimer timer;

  PROCESS_BEGIN();

  static mpu6050_cfg_t cfg;
  static mpu6050_raw_t raw;
  cfg.data_rate = MPU6050_DATARATE_1KHZ;
  cfg.acc_range = MPU6050_ACCELEROMETER_RANGE_2G;
  cfg.gyro_range = MPU6050_GYROSCOPE_RANGE_500S;
  cfg.filter = MPU6050_FILTER_260HZ_0MS;

  mpu6050_init(&g_mpu6050);
  mpu6050_set_cfg(&g_mpu6050,&cfg);
  /* Setup a periodic timer that expires after 10 seconds. */
  etimer_set(&timer, CLOCK_SECOND * 1);

  rlog_i("start up");
  while(1) {
    leds_toggle(LEDS_LED0);

   mpu6050_get_raw(&g_mpu6050,&raw);
    
    rlog_i("ACC %d %d %d",raw.acc_x,raw.acc_y,raw.acc_z);
    rlog_i("gory %d %d %d",raw.gyros_x,raw.gyros_y,raw.gyros_z);
    
    PROCESS_WAIT_EVENT_UNTIL(etimer_expired(&timer));
    etimer_reset(&timer);
  }

  PROCESS_END();
}

